﻿#include "AlgoSNoise.h"
#include <stdexcept>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

using namespace algo;

class algo::SelfNoiseModelPrivate
{
public:
	// Platform parameters
	PlatformParameters params_;

	// --- Motion trajectory interpolation ---
	SubmarineState interpolateState(const SubmarineState& start, const SubmarineState& end, double current_time) const
	{
		double total_duration = end.time - start.time;
		if (total_duration <= 0) {
			return start; // or end, depending on desired behavior at boundary
		}

		double time_ratio = (current_time - start.time) / total_duration;
		double cos_factor = 0.5 * (1.0 - std::cos(M_PI * time_ratio));

		SubmarineState interpolated;
		interpolated.time = current_time;
		interpolated.velocity = start.velocity + (end.velocity - start.velocity) * cos_factor;
		interpolated.depth = start.depth + (end.depth - start.depth) * cos_factor;
		interpolated.heading = start.heading + (end.heading - start.heading) * cos_factor;
		//interpolated.ferq_bins= start.ferq_bins;

		return interpolated;
	}

	// --- Core dynamics and acoustics calculation functions ---
	NoiseTimePoint computeInstantaneousNoise(
		const SubmarineState& current_micro_state,
		const SubmarineState& next_micro_state)
	{
		const auto& ac = params_.acoustics;
		std::vector<double> frequency = ac.operating_freq;
		std::vector<std::pair<double, double>> total_noise_db;
		double dt = next_micro_state.time - current_micro_state.time;
		if (dt <= 1e-6) { // Avoid division by zero for extremely small time intervals
			for (double f : frequency) {
				double noise = -1.0;
				total_noise_db.emplace_back(f, noise);
			}
		}
		else {
			double v1 = current_micro_state.velocity;
			double v2 = next_micro_state.velocity;
			double acceleration = (v2 - v1) / dt;

			// b. Dynamic analysis
			double avg_velocity = (v1 + v2) / 2.0;
			double avg_depth = (current_micro_state.depth + next_micro_state.depth) / 2.0;

			double C_d = calculateDragCoefficient(avg_velocity, avg_depth);
			double drag_force = 0.5 * params_.water_props.density * params_.geometry.cross_section_area * C_d * avg_velocity * avg_velocity;

			// Thrust = Drag force + Inertial force
			double thrust_force = drag_force + params_.geometry.mass * acceleration;
			if (thrust_force < 0) thrust_force = 0; // Thrust cannot be negative

			// Shaft power = (Thrust * Velocity) / Efficiency
			double shaft_power = (thrust_force * avg_velocity) / params_.propulsion.propeller_efficiency;
			if (shaft_power < 0) shaft_power = 0;

			// Estimate propeller rotational speed (RPM) - This is a simplified model; the actual relationship is complex
			// P_shaft ∝ n^3, so n ∝ P_shaft^(1/3)
			// Assume a proportional constant (set to 1.0 here; actual value requires calibration)
			double propeller_rpm = (shaft_power > 0) ? std::pow(shaft_power, 1.0 / 3.0) * 1.0 : 0;

			// c. Acoustic analysis
			double v_cis = calculateCavitationInceptionSpeed(avg_depth);

			std::map<std::string, double> L_M = calculateMachineryNoise(shaft_power, frequency);
			std::map<std::string, double> L_F = calculateFlowNoise(avg_velocity, frequency);
			std::map<std::string, double> L_P1 = calculatePropellerNoise(propeller_rpm, frequency);
			std::map<std::string, double> L_P2 = calculateCavitationNoise(avg_velocity, v_cis, frequency);

			// Energy summation (dB -> Pa² -> Sum -> dB)
			std::map<std::string, double> total_linear_noise_dic;

			for (const auto& pair : L_M) {
				total_linear_noise_dic[pair.first] = std::pow(10, pair.second / 10);
			}
			for (const auto& pair : L_F) {
				total_linear_noise_dic[pair.first] += std::pow(10, pair.second / 10);
			}
			for (const auto& pair : L_P1) {
				total_linear_noise_dic[pair.first] += std::pow(10, pair.second / 10);
			}
			for (const auto& pair : L_P2) {
				total_linear_noise_dic[pair.first] += std::pow(10, pair.second / 10);
			}
			for (const auto& pair : total_linear_noise_dic) {
				total_noise_db.emplace_back(std::stod(pair.first), 10.0 * std::log10(pair.second));
			}
		}
		return { current_micro_state.time, total_noise_db };
	}

	// --- Functional models for key parameters ---
	// Calculate Reynolds number
	double calculateReynoldsNumber(double velocity) const
	{
		return (params_.water_props.density * velocity * params_.geometry.length) / params_.water_props.dynamic_viscosity;
	}

	// Calculate total drag coefficient
	double calculateDragCoefficient(double velocity, double depth) const
	{
		// 1. Frictional resistance
		double Re = calculateReynoldsNumber(velocity);
		double C_d_friction = 0.0;
		if (Re > 1e-6) { // Avoid division by zero
			C_d_friction = params_.hydrodynamics.friction_coeff_base / std::pow(Re, params_.hydrodynamics.friction_exponent);
		}

		// 2. Form drag
		double C_d_form = params_.hydrodynamics.form_drag_coeff;

		// 3. Wave-making resistance
		double C_d_wave = 0.0;
		if (velocity > 1e-6 && depth > 0) {
			double exp_term = -2.0 * params_.env_params.gravity * depth / (velocity * velocity);
			C_d_wave = params_.hydrodynamics.wave_drag_coeff_base * std::exp(exp_term);
		}

		return C_d_friction + C_d_form + C_d_wave;
	}

	// Calculate cavitation inception speed
	double calculateCavitationInceptionSpeed(double depth) const
	{
		const auto& env = params_.env_params;
		const auto& water = params_.water_props;
		const auto& prop = params_.propulsion;

		double numerator = env.atm_pressure + water.density * env.gravity * depth - water.sat_vapor_pressure;
		double denominator = 0.5 * water.density * prop.cavitation_inception_number;

		if (numerator < 0 || denominator <= 1e-6) {
			return std::numeric_limits<double>::max(); // No cavitation possible or invalid params
		}

		return std::sqrt(numerator / denominator);
	}

	// --- Acoustic component calculations ---
	// Calculate machinery noise
	std::map<std::string, double> calculateMachineryNoise(double shaft_power, std::vector<double> freq) const
	{
		const auto& ac = params_.acoustics;
		std::map<std::string, double> freq_mach_noise;
		double noise;
		for (double f : freq) {
			if (shaft_power <= 0) {
				noise = -100;
			}
			else {
				noise = ac.machinery_noise_base_level + ac.machinery_noise_power_factor * std::log10(shaft_power);
				for (int j = 0; j < ac.n_mechanical_tones; ++j) {
					if (std::abs(f - ac.mechanical_freqs[j]) < 1.0) {
						noise += ac.mechanical_levels[j];
					}
				}
			}
			freq_mach_noise.insert({ std::to_string(f), noise });
		}
		return freq_mach_noise;
	}

	// Calculate flow noise
	std::map<std::string, double> calculateFlowNoise(double velocity, std::vector<double> freq) const
	{
		const auto& ac = params_.acoustics;
		double noise;
		std::map<std::string, double> freq_flow_noise;
		for (double f : freq) {
			if (velocity <= 0) {
				noise = -100;
			}
			else {
				noise = ac.flow_noise_base_level + ac.flow_noise_velocity_factor * std::log10(velocity) - ac.flow_noise_slope * log10(f / ac.flow_noise_ref_freq);
			}
			freq_flow_noise.insert({ std::to_string(f), noise });
		}
		return freq_flow_noise;
	}

	// Calculate propeller noise (non-cavitating)
	std::map<std::string, double> calculatePropellerNoise(double propeller_rpm, std::vector<double> freq) const
	{
		const auto& ac = params_.acoustics;
		double noise;
		std::map<std::string, double> freq_Prop_noise;

		for (double f : freq) {
			if (propeller_rpm <= 0) {
				noise = -100.0;
			}
			else {
				noise = ac.prop_noise_base_level + ac.prop_noise_rpm_factor * std::log10(propeller_rpm) + ac.propeller_broadband_slope * log10(f / ac.flow_noise_ref_freq);
				for (int j = 1; j < ac.n_propeller_tones + 1; j++) {
					if (std::abs(f - params_.geometry.leaf_num * j * propeller_rpm) < 0.1) {
						noise += ac.propeller_levels[j];
					}
				}
			}
			freq_Prop_noise.insert({ std::to_string(f), noise });
		}
		return freq_Prop_noise;
	}

	// Calculate propeller cavitation noise
	std::map<std::string, double> calculateCavitationNoise(double velocity, double v_cis, std::vector<double> freq) const
	{
		const auto& ac = params_.acoustics;
		double noise;
		std::map<std::string, double> freq_Cavi_noise;
		for (double f : freq) {
			if (velocity > v_cis) {
				if (f < ac.crossover_freq) {
					noise = ac.cavitation_noise_base_level + ac.cavitation_noise_velocity_factor * (velocity - v_cis)
						+ ac.cavitation_slope_low * log2(f / ac.flow_noise_ref_freq);
				}
				else {
					noise = ac.cavitation_noise_base_level + ac.cavitation_noise_velocity_factor * (velocity - v_cis)
						+ ac.cavitation_slope_low * log2(ac.crossover_freq / ac.flow_noise_ref_freq)
						+ ac.cavitation_slope_high * log2(f / ac.crossover_freq);
				}
			}
			else {
				noise = -100.0;
			}
			freq_Cavi_noise.insert({ std::to_string(f), noise });
		}
		return freq_Cavi_noise;
	}
};

algo::SelfNoiseModel::SelfNoiseModel(const PlatformParameters& params)
	: d(new algo::SelfNoiseModelPrivate)
{
	d->params_ = params;
}

algo::SelfNoiseModel::~SelfNoiseModel()
{
	delete d;
}

std::vector<NoiseTimePoint> algo::SelfNoiseModel::calculateNoiseProfile(
	const SubmarineState& start_state,
	const SubmarineState& end_state,
	double time_step)
{
	if (time_step <= 0) {
		throw std::invalid_argument("Time step must be positive.");
	}

	std::vector<NoiseTimePoint> noise_profile;
	double total_duration = end_state.time - start_state.time;

	if (total_duration < 0) {
		throw std::invalid_argument("End time must be after start time.");
	}
	if (total_duration == 0) {
		// Handle zero duration case if necessary, maybe return single point
		return noise_profile;
	}

	for (double t = start_state.time; t < end_state.time; t += time_step) {
		SubmarineState current_micro_state = d->interpolateState(start_state, end_state, t);
		double next_t = std::min(t + time_step, end_state.time);
		SubmarineState next_micro_state = d->interpolateState(start_state, end_state, next_t);

		NoiseTimePoint instantaneous_noise = d->computeInstantaneousNoise(current_micro_state, next_micro_state);
		noise_profile.push_back(instantaneous_noise);
	}

	return noise_profile;
}
